#include "rclcpp/rclcpp.hpp"
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>


// 重新定义节点类型
struct PointXYZIntensities
{
    float x;
    float y;
    float z;
    float intensities; 
};

POINT_CLOUD_REGISTER_POINT_STRUCT(
    PointXYZIntensities,
    (float, x, x)
    (float, y, y)
    (float, z, z)
    (float, intensities, intensities)
);

class Mult_line_node : public rclcpp::Node
{
public:
  Mult_line_node() : Node("Mult_line_node")
  {
      cloud_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(
                "pointcloud", 10, std::bind(&Mult_line_node::cloud_topic_callback, this, std::placeholders::_1));
      pub_cloud = this->create_publisher<sensor_msgs::msg::PointCloud2>("/moutput_cloud", 10); 
  }

  private:
        void cloud_topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
        {
            pcl::PointCloud<pcl::PointXYZI>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZI>);
            //自定义点类型解析
            pcl::PointCloud<PointXYZIntensities>::Ptr custom_cloud(new pcl::PointCloud<PointXYZIntensities>);
            pcl::fromROSMsg(*msg, *custom_cloud);  

            if (custom_cloud->empty()) {
                    RCLCPP_WARN(this->get_logger(), "Received empty point cloud. Skipping ICP.");
                    return;
            }    
            //laser_cloud->resize(custom_cloud->size());
            laser_cloud->resize(800);
             float max_intensity,min_intensity;
             int max_count,min_count;
           max_intensity=-100;   min_intensity=100; 
           max_count=0;min_count=0;
           // for (size_t i = 0; i < custom_cloud->size(); ++i) 
            for (size_t i = 0; i < 800; ++i) 
            {
            auto& src = custom_cloud->points[i];
            auto& dst = laser_cloud->points[i];
            
            dst.x = src.x;
            dst.y = src.y;
            dst.z = src.z;
            dst.intensity = src.intensities;  // 映射反射率到强度
            if(max_intensity<dst.intensity)max_intensity=dst.intensity;
            if(min_intensity>dst.intensity)min_intensity=dst.intensity;   
            if(dst.intensity<10)min_count++;
            if(dst.intensity>80)max_count++;
            }
             // 转换回ROS消息 
            sensor_msgs::msg::PointCloud2 output_msg;  
            pcl::toROSMsg(*laser_cloud, output_msg);
               // 关键：复制原始消息的header 
            output_msg.header  = msg->header; // 同步frame_id和时间戳
            output_msg.header.stamp  = this->now(); // 或保持原时间戳
            pub_cloud->publish(output_msg);
             
          RCLCPP_INFO(this->get_logger(),"receive data size=%d max_in=%6.3f min_in=%6.3f",
                                custom_cloud->size(),max_intensity,min_intensity); 
           RCLCPP_INFO(this->get_logger(),"max_count=%d min_count=%d",max_count,min_count );                                    
        }
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cloud;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<Mult_line_node>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}